#include <stdio.h>
#include <stdlib.h>
#include <time.h>  // clock(), CLOCKS_PER_SEC
//#include <sys/time.h>  // timeval, gettimeofday()
#include <unistd.h>  // sleep(), usleep()
#include <signal.h>  // signal()

#include <iostream>
//#include <fstream>
#include <iomanip>  // io manipulator, set cin or cout format, std::fixed, std::setprecision()
#include <string>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>  // pcl::io::loadPCDFile(), pcl::io::savePCDFile()
#include <pcl/octree/octree.h>
#include <pcl/filters/statistical_outlier_removal.h>  // pcl::StatisticalOutlierRemoval
#include <pcl/filters/voxel_grid.h>  // pcl::VoxelGrid
#include <pcl/common/transforms.h>  // pcl::transformPointCloud()

#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>  // pcl::toROSMsg(), pcl::fromROSMsg()

#include "cld_reg.h"

using PointType = pcl::PointXYZI;

void adjustRaw(pcl::PointCloud<PointType>& cld_src, pcl::PointCloud<PointType>& cld_tgt) {
	clock_t t1 = clock();

	const size_t sz_src = cld_src.points.size();

	if (&cld_tgt != &cld_src) {
		cld_tgt.header = cld_src.header;
		cld_tgt.points.resize(sz_src);
	}

	// kunming0822
	const float x_min = -43.0f - 20.0f;
	const float x_max = 76.0f + 20.0f;
	const float y_min = -115.0f - 20.0f;
	const float y_max = 40.0f + 20.0f;  // 20.0f + 20.0f
	const float z_min = -6.0f - 2.0f;
	const float z_max = 8.0f + 2.0f;

	size_t j = 0;
	float x, y, z;
	for (size_t i = 0; i < sz_src; i++) {
		x = cld_src.points[i].x;
		y = cld_src.points[i].y;
		z = cld_src.points[i].z;

		// kunming0822
		if (x < x_min || x > x_max || y < y_min || y > y_max || z < z_min || z > z_max) continue;

		cld_tgt.points[j] = cld_src.points[i];
		j++;
	}
	cld_tgt.points.resize(j);
	cld_tgt.height = 1;
	cld_tgt.width = j;
	cld_tgt.is_dense = true;
	std::cout << sz_src << " -> " << j << std::endl;

	std::cout << std::fixed << std::setprecision(3) << "----adjustRaw (ms): " << double(clock() - t1) * 1000.0 / CLOCKS_PER_SEC << std::endl << std::endl;
}

void adjustTrans(pcl::PointCloud<PointType>& cld_src, pcl::PointCloud<PointType>& cld_tgt) {
	clock_t t1 = clock();

	const size_t sz_src = cld_src.points.size();

	if (&cld_tgt != &cld_src) {
		cld_tgt.header = cld_src.header;
		cld_tgt.points.resize(sz_src);
	}

	// kunming0822
	const float x_min = -43.0f;
	const float x_max = 76.0f;
	const float y_min = -115.0f;
	const float y_max = 40.0f;  // 20.0f
	const float z_min = -6.0f;
	const float z_max = 8.0f;

	size_t j = 0;
	float x, y, z;
	for (size_t i = 0; i < sz_src; i++) {
		x = cld_src.points[i].x;
		y = cld_src.points[i].y;
		z = cld_src.points[i].z;

		// kunming0822
		if (x < x_min || x > x_max || y < y_min || y > y_max || z < z_min || z > z_max) continue;
		if (x >= -43 && x <= -33 && y >= 10 && y <= 20) continue;  // kunming0822h
		if (x >= 22 && x <= 38 && y >= 29 && y <= 40) continue;  // kunming0822h

		cld_tgt.points[j] = cld_src.points[i];
		j++;
	}
	cld_tgt.points.resize(j);
	cld_tgt.height = 1;
	cld_tgt.width = j;
	cld_tgt.is_dense = true;
	std::cout << sz_src << " -> " << j << std::endl;

	std::cout << std::fixed << std::setprecision(3) << "----adjustTrans (ms): " << double(clock() - t1) * 1000.0 / CLOCKS_PER_SEC << std::endl << std::endl;
}

void calVoxelMap(pcl::PointCloud<PointType>::Ptr& cldptr_src, const double resolution, pcl::PointCloud<PointType>& cld_tgt) {
	clock_t t1 = clock();

	const size_t sz_src = cldptr_src->size();

	pcl::VoxelGrid<PointType> vg;
	vg.setLeafSize(resolution, resolution, resolution);
	vg.setInputCloud(cldptr_src);
	vg.filter(cld_tgt);
	std::cout << sz_src << " -> " << cld_tgt.size() << std::endl;

	std::cout << std::fixed << std::setprecision(3) << "----calVoxelMap (ms): " << double(clock() - t1) * 1000.0 / CLOCKS_PER_SEC << std::endl << std::endl;
}

void calGridMap(pcl::PointCloud<PointType>::Ptr& cldptr_src, const double resolution, pcl::PointCloud<PointType>& cld_tgt) {
	clock_t t1 = clock();

	const size_t sz_src = cldptr_src->size();
	if (sz_src == 0) {
		std::cout << "calGridMap(): input cld error" << std::endl;
		return;
	}

	Eigen::Vector4f v4_min, v4_max;
	pcl::getMinMax3D(*cldptr_src, v4_min, v4_max);
	//std::cout << v4_min.transpose() << "  -  ";
	//std::cout << v4_max.transpose() << std::endl;

	float data[6];
	data[0] = std::abs(v4_min(0));
	data[1] = std::abs(v4_min(1));
	data[2] = std::abs(v4_min(2));
	data[3] = std::abs(v4_max(0));
	data[4] = std::abs(v4_max(1));
	data[5] = std::abs(v4_max(2));
	double r_max = 0;
	for (size_t i = 0; i < 6; ++i) {
		if (data[i] > r_max) r_max = data[i];
	}
	double r = resolution;
	while (r <= r_max) {
		r *= 2;
	}
	std::cout << resolution << ", " << r_max << ", " << r << std::endl;

	pcl::octree::OctreePointCloudOccupancy<PointType> oct_tgt(resolution);
	oct_tgt.defineBoundingBox(-r_max, -r_max, -r_max, r_max, r_max, r_max);
	oct_tgt.setInputCloud(cldptr_src);
	oct_tgt.addPointsFromInputCloud();

	//double x_min, y_min, z_min, x_max, y_max, z_max;
	//oct_tgt.getBoundingBox(x_min, y_min, z_min, x_max, y_max, z_max);
	//std::cout << x_min << ", " << y_min << ", " << z_min << "  -  ";
	//std::cout << x_max << ", " << y_max << ", " << z_max << std::endl;

	size_t k = oct_tgt.getLeafCount();
	cld_tgt.resize(k);
	cld_tgt.height = 1;
	cld_tgt.width = k;
	cld_tgt.is_dense = true;
	size_t i = 0;
	Eigen::Vector3f voxel_min, voxel_max, voxel_center;
	for (auto it = oct_tgt.leaf_begin(); it != oct_tgt.leaf_end(); ++it) {
		oct_tgt.getVoxelBounds(it, voxel_min, voxel_max);
		voxel_center = (voxel_min + voxel_max) * 0.5;
		cld_tgt.points[i].x = voxel_center(0);
		cld_tgt.points[i].y = voxel_center(1);
		cld_tgt.points[i].z = voxel_center(2);
		i++;
	}
	std::cout << sz_src << " -> " << k << std::endl;

	std::cout << std::fixed << std::setprecision(3) << "----calGridMap (ms): " << double(clock() - t1) * 1000.0 / CLOCKS_PER_SEC << std::endl << std::endl;
}

void refineCld(pcl::PointCloud<PointType>::Ptr& cldptr_src, pcl::PointCloud<PointType>& cld_tgt) {
	clock_t t1 = clock();

	const size_t sz_src = cldptr_src->size();

	pcl::StatisticalOutlierRemoval<PointType> sor;
	sor.setInputCloud(cldptr_src);
	sor.setMeanK(10);
	sor.setStddevMulThresh(1.0);
	sor.setNegative(false);  // true for outlier, false for inlier
	sor.filter(cld_tgt);
	std::cout << sz_src << " -> " << cld_tgt.size() << std::endl;

	std::cout << std::fixed << std::setprecision(3) << "----refineCld (ms): " << double(clock() - t1) * 1000.0 / CLOCKS_PER_SEC << std::endl << std::endl;
}

void adjustDepot(pcl::PointCloud<PointType>& cld_src, pcl::PointCloud<PointType>& cld_tgt) {
	clock_t t1 = clock();

	const size_t sz_src = cld_src.points.size();

	if (&cld_tgt != &cld_src) {
		cld_tgt.header = cld_src.header;
		cld_tgt.points.resize(sz_src);
	}

	size_t j = 0;
	float x, y, z;
	for (size_t i = 0; i < sz_src; i++) {
		x = cld_src.points[i].x;
		y = cld_src.points[i].y;
		z = cld_src.points[i].z;

		// kunming0822
		// roof
		if (z > 2) {
			continue;
		}
		// area 0
		if (x > -31.8 && x < 43.8 && y > -112.7 && y < -91.9) {
			continue;
		}
		// area 1
		if (x > -41.7 && x < 34.3 && y > -90.5 && y < -69.9) {
			continue;
		}
		// area 2
		if (x > -41.7 && x < 29.6 && y > -68.7 && y < -58.9) {
			continue;
		}
		// area 3
		if (x > -41.7 && x < 25.1 && y > -58.0 && y < -48.0) {
			continue;
		}
		// area 4
		if (x > -37.0 && x < 20.4 && y > -46.7 && y < -36.7) {
			continue;
		}
		// area 5
		if (x > -32.2 && x < 15.9 && y > -35.9 && y < -26.1) {
			continue;
		}
		// area 6
		if (x > -27.2 && x < 11.1 && y > -24.6 && y < -14.7) {
			continue;
		}
		// area 7
		if (x > -22.6 && x < 6.5 && y > -14.1 && y < -4.0) {
			continue;
		}
		// area 8
		//if (x > -13.0 && x < -2.5 && y > -2.9 && y < 17.9) {
		//	continue;
		//}

		cld_tgt.points[j] = cld_src.points[i];
		j++;
	}
	cld_tgt.points.resize(j);
	cld_tgt.height = 1;
	cld_tgt.width = j;
	cld_tgt.is_dense = true;
	std::cout << sz_src << " -> " << j << std::endl;

	std::cout << std::fixed << std::setprecision(3) << "----adjustDepot (ms): " << double(clock() - t1) * 1000.0 / CLOCKS_PER_SEC << std::endl << std::endl;
}

pid_t getPidbyName(const char *pname) {
	FILE *fp;
	char buf[256] = {0};
	char cmd[256] = {0};
	pid_t pid = -1;
	sprintf(cmd, "pidof %s", pname);
	if((fp = popen(cmd, "r")) != NULL) {
		if(fgets(buf, sizeof(buf), fp) != NULL) {
			pid = atoi(buf);
		}
	}
	pclose(fp);
	return pid;
}

int main(int argc, char **argv) {
	ros::init(argc, argv, "post_mapping_node");
	ros::NodeHandle nh("~");

	ros::Publisher pub_prev = nh.advertise<sensor_msgs::PointCloud2>("/slamPrevMap_topic", 1);
	ros::Publisher pub_raw = nh.advertise<sensor_msgs::PointCloud2>("/slamRawMap_topic", 1);
	ros::Publisher pub_vox = nh.advertise<sensor_msgs::PointCloud2>("/slamVoxelMap_topic", 1);
	ros::Publisher pub_grid = nh.advertise<sensor_msgs::PointCloud2>("/slamGridMap_topic", 1);

	const double resolution = 0.2;

	//const std::string dname = "/home/sany/catkin_slam/src/fast_lio/PCD/";
	const std::string dname = std::string(ROOT_DIR) + "PCD/";

	const std::string fname_prev = dname + "0822kunming02h.pcd";
	const std::string fname_raw = dname + "scans.pcd";
	const std::string fname_vox = dname + "voxel_map.pcd";
	const std::string fname_grid = dname + "grid_map.pcd";

	const char* pname = "fastlio_mapping_node";

	// ----------
	pid_t pid = getPidbyName(pname);
	if (pid < 0) {
		std::cout << "warning: can't get pid of " << pname << std::endl << std::endl;
	}
	else {
		std::cout << pname << ", pid = " << pid << std::endl;
		if (kill(pid, SIGINT) == -1) {
			std::cout << "error: can't send signal to " << pid << std::endl;
			exit(0);
		}
		else {
			std::cout << "waiting for saving pcd file ..." << std::endl << std::endl;
			//sleep(300);
		}
	}

	// ----------
	pcl::PointCloud<PointType>::Ptr cldptr_prev(new pcl::PointCloud<PointType>);
	pcl::PointCloud<PointType>::Ptr cldptr_raw(new pcl::PointCloud<PointType>);
	pcl::PointCloud<PointType>::Ptr cldptr_trans(new pcl::PointCloud<PointType>);
	pcl::PointCloud<PointType>::Ptr cldptr_vox(new pcl::PointCloud<PointType>);
	pcl::PointCloud<PointType>::Ptr cldptr_grid(new pcl::PointCloud<PointType>);

	clock_t t1;

	std::cout << "fname_prev: " << fname_prev << std::endl;
	t1 = clock();
	if (pcl::io::loadPCDFile<PointType>(fname_prev, *cldptr_prev) == -1) {
		std::cout << "load prev pcd error" << std::endl;
		exit(0);
	}
	std::cout << "cldptr_prev size: " << cldptr_prev->size() << std::endl;
	std::cout << std::fixed << std::setprecision(3) << "----load prev map (ms): " << double(clock() - t1) * 1000.0 / CLOCKS_PER_SEC << std::endl << std::endl;

	std::cout << "fname_raw: " << fname_raw << std::endl;
	t1 = clock();
	if (pcl::io::loadPCDFile<PointType>(fname_raw, *cldptr_raw) == -1) {
		std::cout << "load raw pcd error" << std::endl;
		exit(0);
	}
	std::cout << "cldptr_raw size: " << cldptr_raw->size() << std::endl;
	std::cout << std::fixed << std::setprecision(3) << "----load raw map (ms): " << double(clock() - t1) * 1000.0 / CLOCKS_PER_SEC << std::endl << std::endl;

	// cut raw
	//adjustRaw(*cldptr_raw, *cldptr_raw);

	// transform
	cldreg::Reg cld_reg(cldptr_prev, 1.0, 10);
	Eigen::Matrix4f mat_tf;
	cld_reg.getFinalTransformation(cldptr_raw, mat_tf);
	//
	t1 = clock();
	pcl::transformPointCloud(*cldptr_raw, *cldptr_trans, mat_tf);
	std::cout << std::fixed << std::setprecision(3) << "----transformPointCloud (ms): " << double(clock() - t1) * 1000.0 / CLOCKS_PER_SEC << std::endl << std::endl;

	// cut trans
	adjustTrans(*cldptr_trans, *cldptr_trans);

	// cld -> vox map
	calVoxelMap(cldptr_raw, resolution, *cldptr_raw);
	calVoxelMap(cldptr_trans, resolution, *cldptr_vox);
	// sor
	//refineCld(cldptr_vox, *cldptr_vox);
	// save pcd
	pcl::io::savePCDFile(fname_vox, *cldptr_vox);
	std::cout << "save vox map fin" << std::endl << std::endl;

	// cld -> grid map
	calGridMap(cldptr_trans, resolution, *cldptr_grid);
	// sor
	refineCld(cldptr_grid, *cldptr_grid);
	// cut depot
	adjustDepot(*cldptr_grid, *cldptr_grid);
	// save pcd
	pcl::io::savePCDFile(fname_grid, *cldptr_grid);
	std::cout << "save grid map fin" << std::endl << std::endl;

	// ----------
	/*sensor_msgs::PointCloud2 msg_prev;
	pcl::toROSMsg(*cldptr_prev, msg_prev);
	msg_prev.header.frame_id = "map";

	sensor_msgs::PointCloud2 msg_raw;
	pcl::toROSMsg(*cldptr_raw, msg_raw);
	msg_raw.header.frame_id = "map";

	sensor_msgs::PointCloud2 msg_vox;
	pcl::toROSMsg(*cldptr_vox, msg_vox);
	msg_vox.header.frame_id = "map";

	sensor_msgs::PointCloud2 msg_grid;
	pcl::toROSMsg(*cldptr_grid, msg_grid);
	msg_grid.header.frame_id = "map";

	ros::Rate rate(5);
	while (ros::ok()) {
		ros::spinOnce();
		pub_prev.publish(msg_prev);
		pub_raw.publish(msg_raw);
		pub_vox.publish(msg_vox);
		pub_grid.publish(msg_grid);
		rate.sleep();
	}*/

	std::cout << "main() fin" << std::endl;
	return 0;
}

